/* Copyright 2001,2002,2003 NAH6
 * All Rights Reserved
 *
 * Parts Copyright DoD, Parts Copyright Starium
 *
 */
/*#include "filter.h"
#include "main.h"	not needed with celpfilt.h */
#include "celpfilt.h"
#include "adapt.h"
#include "lp_anal.h"
#include "bwexp.h"
#include "analysis.h"
#include "con_adap.h"
#include "interp.h"
#include "lsftopc.h"
#include "mexcite.h"
#include "setarray.h"
#include "stoch.h"

static void CreateSubFrame(
float	FutureSpeech[F_LEN],
float	PresentSpeech[F_LEN]);

static void FindImpulseResponse(
float	pc[ORDER+1],
float	h[SF_LEN]);

static void FindLPResidual(
float	speech[SF_LEN],
float	pc[ORDER+1],
float	residual[RES_LEN]);

static void UpdateEverything(
float	OptExcVec[SF_LEN],
float	Speech[SF_LEN],
float	AdaptCB[ACB_SIZE],
float	AdaptGain,
float	AdaptDelay,
float	pc[ORDER+1]);

/**************************************************************************
*                                                                         *
* ROUTINE
*		analysis
*
* FUNCTION
*		CELP analysis
* SYNOPSIS
*		analysis(SpeechIn, parameters, frame_num)
*
*   formal
*
*                       data    I/O
*       name            type    type    function
*       -------------------------------------------------------------------
*	SpeechIn	float	 i	Frame of input speech
*	parameters	TX_PARAM o	Frame of CELP parameters
*	frame_num	int	 i	Frame number (for diagnostics)
*
**************************************************************************
*
* DESCRIPTION
*
*	This performs the analysis of the input speech and produces
*	a set of parameters that are sent to the synthesizer for
*	reconstruction.
*
**************************************************************************
*
* CALLED BY
*
*	celp.c
*
* CALLS
*
*	HPF_InSpeech LP_Analysis Interpolate CreateSubFrame 
*
*	loop around:
*	LSFtoPC FindLPResidual FindImpulseResponse AdaptiveAnalysis
*	ModifiedExcitation StochasticAnalysis UpdateEverything
*
**************************************************************************/
void Analysis(
float 	SpeechIn[F_LEN],
TX_PARAM *parameters,
int	frame_num)
{
	float 		LSFint[NUM_SF][ORDER], pc[ORDER+1];
	int		sf;
	float		LP_res[RES_LEN], Adaptive_res[RES_LEN];
	float		LPImpulseResponse[SF_LEN];
	float		PresentSpeech[F_LEN];
	float		scale;
static 	float		AdaptiveCB[ACB_SIZE];
	float		OptimumExcitation[SF_LEN];


/*  High Pass Filter Input Speech */
	HPF_InSpeech(SpeechIn);

/*  LP Analysis of Input speech */
	LP_Analysis(SpeechIn, parameters->lsf, frame_num);

/*  Interpolate LSPs */
	Interpolate(parameters->lsf, LSFint, ANALYSIS, frame_num);

/*  Create vector of "present" speech for codebook searches */
	CreateSubFrame(SpeechIn, PresentSpeech);

/*  Loop through subframes to find codebook (adaptive and stochastic) indices */
	for (sf = 0; sf < NUM_SF; sf++) {

/*  Convert quantized, interpolated LSFs to PCs */
	  LSFtoPC(LSFint[sf], pc, frame_num);

/*  Find the residual from LP analysis using the quantized, interpolated LSFs */
	  FindLPResidual(&PresentSpeech[sf*SF_LEN], pc, LP_res);

/*  Find Impulse Response response of PCs */
	  FindImpulseResponse(pc, LPImpulseResponse); 

/*  Use the LP residual to find the correct adaptive codebook index and gain */
 	  AdaptiveAnalysis(&PresentSpeech[sf*SF_LEN], pc, LPImpulseResponse, AdaptiveCB,
		LP_res, &parameters->AdaptiveDelay[sf], 
		&parameters->AdaptiveGain[sf], Adaptive_res);
 
/*  Calculate the appropriate scaling for the Stochastic Analysis */
	  ModifiedExcitation(LP_res, Adaptive_res, &scale);

/*  Calculate the correct stochastic codebook index and gain */  
  	  StochasticAnalysis(scale, LPImpulseResponse, Adaptive_res,  
		&parameters->StochasticIndex[sf], 
		&parameters->StochasticGain[sf], OptimumExcitation);

/*  Update adaptive codebook and residual filters for next subframe */
	  UpdateEverything(OptimumExcitation, &PresentSpeech[sf*SF_LEN], AdaptiveCB,
		parameters->AdaptiveGain[sf], parameters->AdaptiveDelay[sf], pc);
	}

}
/*

*************************************************************************
*                                                                         *
* ROUTINE
*		CreateSubFrame
*
* FUNCTION
*		Create subframe vector from past and current samples
* SYNOPSIS
*		CreateSubFrame(FutureSpeech, PresentSpeech)
*
*   formal
*
*                       data    I/O
*       name            type    type    function
*       -------------------------------------------------------------------
*	FutureSpeech	float	 i	Frame of future speech
*	PresentSpeech	flaot	 o	Frame of present speech
*
**************************************************************************/

void CreateSubFrame(
float	FutureSpeech[F_LEN],
float	PresentSpeech[F_LEN])
{
static float	PastSpeech[F_LEN/2];
static int	first=TRUE;
int		i;

/*  Initialize past speech on first pass through */
	if (first)	{
	  first = FALSE;
	  for(i=0;i<F_LEN/2;i++)
	    PastSpeech[i] = 0.0;
	}

/*  Create vector of present speech from past and future speech */
	for(i=0;i<F_LEN/2;i++)	{
	  PresentSpeech[i] = PastSpeech[i];
	  PresentSpeech[i+F_LEN/2] = FutureSpeech[i];
	}

/*  Save remaining future speech for past speech in the next frame */
	for(i=0;i<F_LEN/2;i++)	{
	  PastSpeech[i] = FutureSpeech[i+F_LEN/2];
	}

/*  I suppose the 2 previous loops could be combined for better speed,
	but 2 separate loops may be easier to understand ... 		*/

}

/*

*************************************************************************
*                                                                         *
* ROUTINE
*		FindImpulseResponse
*
* FUNCTION
*		Compute impulse response with direct form filter
* SYNOPSIS
*		FindImpulseResponse(pc, h)
*
*   formal
*
*                       data    I/O
*       name            type    type    function
*       -------------------------------------------------------------------
*	pc		float	 i	Frame of Predictor Coefficients
*	h		float	 o	Impulse response
*
**************************************************************************/

void FindImpulseResponse(
float	pc[ORDER+1],
float	h[SF_LEN])
{
float	pcexp[ORDER+1];

/*  Initialize impulse response array */
	SetArray(SF_LEN, 0.0, h);
	h[0] = 1.0;

/*  Bandwidth expand input predictor coefficients */
	BWExpand(GAMMA, pc, pcexp);

/*  Filter h with expanded PCs (no past history here) */
	FilterImpulseResponse(h, pcexp);


}

/*

*************************************************************************
*                                                                         *
* ROUTINE
*		FindLPResidual
*
* FUNCTION
*		Compute residual from lpc systhesis
* SYNOPSIS
*		FindLPResidual(pc, residual)
*
*   formal
*
*                       data    I/O
*       name            type    type    function
*       -------------------------------------------------------------------
*	speech		float	 i	Subframe of input speech
*	pc		float	 i	Frame of Predictor Coefficients
*	residual	float	 o	Residual from LP analysis
*
**************************************************************************/

void FindLPResidual(
float	speech[SF_LEN],
float	pc[ORDER+1],
float	residual[RES_LEN])
{
int	i;
float	pcexp[ORDER+1];

/*  Calculate zero input response  */
	SetArray(RES_LEN, 0.0, residual);
	do_pfilt_dynamic(&LP_ResP, pc, residual);

/*  Calculate Difference between zero input response and original speech */
	for(i=0;i<RES_LEN;i++)	{
	  residual[i] = speech[i] - residual[i];
	}

/*  Perceptual Weighting of the difference */
	do_zfilt_dynamic(&LP_ResZ, pc, residual);
	BWExpand(GAMMA, pc, pcexp);
	do_pfilt_dynamic(&LP_ResP2, pcexp, residual);
}

/*

*************************************************************************
*                                                                         *
* ROUTINE
*		UpdateEverything
*
* FUNCTION
*		Update adaptive parameters for next subframe
* SYNOPSIS
*		UpdateEverything(OptExcVec, Speech AdaptCB, AdaptGain, AdaptDelay, pc)
*
*   formal
*
*                       data    I/O
*       name            type    type    function
*       -------------------------------------------------------------------
*	OptExcVec	float	 i	Optimial excitation vector from
*					stochastic analysis
*	Speech		float	 i	Subframe of present speech
*	AdaptCB		float	i/o	Adaptive codebook vector
*	AdaptGain	float	 i	Adaptive codebook gain
*	AdaptDelay	float	 i	Adaptive codebook delay
*	pc		float	 i	Subframe of interpolated predictor 
*					coefficients
*
**************************************************************************/

void UpdateEverything(
float	OptExcVec[SF_LEN],
float	Speech[SF_LEN],
float	AdaptCB[ACB_SIZE],
float	AdaptGain,
float	AdaptDelay,
float	pc[ORDER+1])
{
	int 	i;
	float	pcexp[ORDER+1];
static	int	first=TRUE;

/*  Initialization */
	if (first)	{
	  first = FALSE;
	  for(i=0;i<ACB_SIZE;i++)
	    AdaptCB[i] = 0.0;
	}

/*  Update Adaptive Codebook vector */
	ConstructAdaptCW(OptExcVec, SF_LEN, AdaptCB, ACB_SIZE, AdaptGain, 
		AdaptDelay, "long");

/*  Update Residuals */
/*  LP Filter the optimal excitation vector */
	do_pfilt_dynamic(&Update_ResP, pc, OptExcVec);

/*  Calculate Residual */
	for(i=0;i<RES_LEN;i++)	{
	  OptExcVec[i] = Speech[i] - OptExcVec[i];
	}

/*  Perceptual Weighting of residual */
	do_zfilt_dynamic(&Update_ResZ, pc, OptExcVec);
	BWExpand(GAMMA, pc, pcexp);
	do_pfilt_dynamic(&Update_ResP2, pcexp, OptExcVec);

/*  Assign updated memories to residual filters */
	LP_ResZ = Update_ResZ; 
	LP_ResP = Update_ResP; 
	LP_ResP2 = Update_ResP2;

	Adapt_ResZ = Update_ResZ; 
	Adapt_ResP = Update_ResP; 
	Adapt_ResP2 = Update_ResP2;

}
